/*
 * Copyright (c) 2011 Sapienza University of Rome.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright
 *   notice, this list of conditions and the following disclaimer in the
 *   documentation and/or other materials provided with the
 *   distribution.
 * - Neither the name of the Sapienza University of Rome nor the names of
 *   its contributors may be used to endorse or promote products derived
 *   from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL SAPIENZA
 * UNIVERSITY OF ROME OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @author Ugo Colesanti <colesanti@dis.uniroma1.it>
 * @version 1.02 (October 12, 2011)
 *
 */

#include "Manager.h"

module ManagerC{
  provides{
	  interface StdControl ;
	  interface ProtocolScheduler ;
	  interface ScheduleManagement ;
	  interface Send ;
	  interface Receive ;
	  interface Reset ;
	  interface DISSenseSampling ;
	  interface DISSenseStatistics ;
  }
  uses{
	  interface StdControl as RoutingControl ;

	  interface Timer<TMilli> as GeneralTimer ;
	  interface Timer<TMilli> as ProtocolTimer ;
	  interface Timer<TMilli> as ShowResyncTimer ;
	  interface Timer<TMilli> as SyncWaitTimer ;
	  interface Timer<TMilli> as DISSenseSamplingTimer ;

	  interface LplResync ;
	  interface AdaptiveLayer ;
	  interface NtpSynchronization ;

	  interface Send as SubSend ;
	  interface Receive as SubReceive ;
//	  interface AMSend as LplStateSender ; debug only was here in august

	  interface CollectionPacket ;
	  interface CtpInfo ;
	  interface RootControl ;

	  interface RMControl ;
	  interface RMSynchronization ;

	  interface Reboot ;

#ifndef TOSSIM
	  interface LowPowerListening ;
#endif
#ifdef TOSSIM
	  interface Random ;
#endif

	  interface Leds ;
  }
}

implementation{

	static void change_state(uint8_t state) ;
	static void restore_previous_state() ;

	static void goto_sleep() ;
	static bool isInSync(uint8_t state) ;
	static void fatal_problem(uint8_t) ;
//	static void addCheckPoint(uint8_t,uint32_t) ;
//	static void printStateAndCheck() ;

	static void sendPacket() ;
	static void showResynchronized() ;

	enum{
		ALPHA = 9 ,
		RISING_ALPHA = 7,
		FALLING_ALPHA = 9,
	};

	bool need_resync ;		// true if waiting for resync, false otherwise

	managerStateMsg_t localState ;	// state struct (all nodes)


	// utility variables for manager (all nodes)
	message_t* msg_buffer ;
	message_t msg_state_buffer ;
	uint8_t msg_len = 0 ;
	uint8_t msg_error ;

	bool sendbusy = FALSE ;
	bool msg_sent = FALSE ;

	uint16_t ttr ;					// maximum time to resync	(root only)
	uint16_t ttrd ;					// time to receive data		(root only)
	uint32_t first_packet ;			// timestamp of the first packet received	(root only)
	uint32_t last_packet ;			// timestamp of last packet received		(root only)

	uint32_t power_on_time ;		// timestamp related to the beginning of the active phase (GT)  (no root)
	uint32_t resync_time_elapsed ; // time elapsed between power_on_time and the resync signal		(no root)


	// metrics
//	uint32_t latency_time ;
	uint16_t resync_counter = 0 ;

	// new with DISSense Manager
	schedule_info_t scheduleInfo ;

	uint32_t sampling_offset = 0 ;

//	uint32_t timer_drift = 0 ;
//	uint32_t timer_adjustment = 0 ;


	/*
	 * Fixed drift: Random.rand % 50*interval*countdown_to_resync*1024
	 * variable drift: Random.rand % 5*interval*countdown_to_resync*1024
	 * fixed_positive: Random.rand16 % 2 -> if true the drift is added, if false it is subtracted
	 * variable_positive: Random.rand16 % 2 -> if true the drift is added, if false it is subtracted
	 */
#ifdef TOSSIM
	uint16_t fixed_drift ;
	uint16_t variable_drift ;
	bool fixed_drift_to_add ;
	bool variable_drift_to_add ;
	uint8_t variable_resync_drift ;
#endif

	event void ShowResyncTimer.fired(){
		call Leds.led0Off() ;

	}

	command error_t StdControl.start(){
		scheduleInfo.current_sampling_period = 0 ;		// 2
		scheduleInfo.current_schedule_gt = 0;			// 2
		scheduleInfo.current_schedule_ri = 0;			// 2
		scheduleInfo.current_schedule_dci = 0;			// 2
		scheduleInfo.current_schedule_skip = 0;			// 1
		scheduleInfo.next_sampling_period = 0;			// 2
		scheduleInfo.time_to_next_sampling_period = 0 ;     // 4
		scheduleInfo.next_sampling_period_fired = 0;	// 4

		dbg("Manager","Starting Manager...\n") ;


		call ScheduleManagement.setCurrentSamplingPeriod(DEFAULT_WAKEUP_INTERVAL) ;
		call ScheduleManagement.setNextSamplingPeriod(DEFAULT_WAKEUP_INTERVAL) ;

#ifdef TOSSIM
		fixed_drift = call Random.rand32() % 50UL ;
		fixed_drift_to_add = call Random.rand16() % 2U ;
#endif

//#ifndef TOSSIM
//		call Notify.enable();
//#endif

		if (TOS_NODE_ID == 0 /* || TOS_NODE_ID == 1*/) call RootControl.setRoot();

		if(call RootControl.isRoot()){
//			printf("Boot node - %u\n",TOS_NODE_ID) ;
			change_state(ROOTSTATE_ACTIVE) ;
			call AdaptiveLayer.initialize(0, CTPTREESETUP_TIMEOUT , PREPARETOSLEEP_TIMEOUT , DEFAULT_WAKEUP_INTERVAL , 300 , 300, 0) ; // different approach: start from low interval and let DISSense reach the good ones.
			if (call RoutingControl.start() != SUCCESS) fatal_problem(FE_BOOT_ROUTINGNOSTART) ;

			call GeneralTimer.startOneShot(call ScheduleManagement.getCurrentSamplingPeriodMilli()) ;
			scheduleInfo.next_sampling_period_fired = call GeneralTimer.getNow() + (call ScheduleManagement.getCurrentSamplingPeriodMilli())*(call AdaptiveLayer.getCurrentSkip()+1) ;
			call ProtocolTimer.startOneShot((uint32_t)call AdaptiveLayer.getCurrentGT()+(uint32_t)call AdaptiveLayer.getCurrentRI()+(uint32_t)call AdaptiveLayer.getCurrentDCI()) ;
			scheduleInfo.current_schedule_gt = call AdaptiveLayer.getCurrentGT() ;
			scheduleInfo.current_schedule_ri = call AdaptiveLayer.getCurrentRI() ;
			scheduleInfo.current_schedule_dci = call AdaptiveLayer.getCurrentDCI() ;
			scheduleInfo.current_schedule_skip = call AdaptiveLayer.getCurrentSkip() ;

//			printf("General Timer will fire in: %lu , ProtocolTimer in %lu\n",call ScheduleManagement.getCurrentSamplingPeriodMilli(),(uint32_t)call AdaptiveLayer.getCurrentGT()+(uint32_t)call AdaptiveLayer.getCurrentRI()+(uint32_t)call AdaptiveLayer.getCurrentDCI()) ;
		}
		else{
//#ifndef TOSSIM
//			change_state(BOOT_MANUAL) ;
//			printf("Boot node - %u - Manual mode\n",TOS_NODE_ID) ;
//#else
//			dbg("Manager","Booting in TRY_RESYNC...\n") ;

			// set the duty cycle to 0.1%
#ifndef TOSSIM
//		  call LowPowerListening.setLocalDutyCycle(10) ;
			call LowPowerListening.setLocalWakeupInterval(4463) ;
#endif

			change_state(PROTOSTATE_LPL_TRY_RESYNC) ;
			call LplResync.beginResyncProcess() ;
//#endif
		}
//#ifndef TOSSIM
//		printfflush() ;
//#endif
		return SUCCESS ;
	}

	command error_t StdControl.stop(){
		return SUCCESS ;
	}

	// Send interface
	command error_t Send.cancel(message_t* msg){
		return call SubSend.cancel(msg) ;
	}

	command void* Send.getPayload(message_t* msg, uint8_t len){
		return call SubSend.getPayload(msg,len+sizeof(rm_stat_t)) ;
	}

	command uint8_t Send.maxPayloadLength(){
		return call SubSend.maxPayloadLength() ;
	}

	command error_t Send.send(message_t* msg, uint8_t len){

		if(msg_len != 0){
			return FAIL ;
		}
		else{
			msg_buffer = msg ;
			msg_len = len ;
			return SUCCESS ;
		}

	}

	command void Reset.reset(uint16_t addr){
		call Reboot.propagate(addr) ;
	}

	event void SubSend.sendDone(message_t* msg,error_t error){
		if(msg != msg_buffer) fatal_problem(FE_SENDDONE_MSGNOTINBUFFER) ;
		msg_error = error ;
		sendbusy = FALSE ;
		msg_sent = TRUE ;
	}

	// Receive interface
	event message_t* SubReceive.receive(message_t* msg , void* payload , uint8_t len){
		uint32_t now ;
		rm_stat_t* rmf = (rm_stat_t*) (payload+len-sizeof(rm_stat_t)) ;
		dbg("Manager","Received packet from %u , time: %s\n",call CollectionPacket.getOrigin(msg),sim_time_string()) ;
		dbg("Manager-Stat-Ttr","Ttr: %u\n",rmf->ttr) ;
		now = call GeneralTimer.getNow() ;
		if(rmf->ttr != 0xffff && rmf->ttr > ttr) ttr = rmf->ttr ;
		if(first_packet == 0) first_packet = now ;
		last_packet = now ;
//	}
//			dbg("DEBUG_TTRD","Adjusting for correct ttrd -> ttr: %u\n",rmf->ttr) ;
//			now -= (uint32_t)rmf->ttr ; no longer required: it was used when there was early sending.
//			if(ttr == 0 || (ttr != 0 && ttr < rmf->ttr)) ttr = rmf->ttr ; puah :S embedded in the first check
//	}

//		if((now < first_packet) || (first_packet == 0)) first_packet = now ;
//		if((now > last_packet) || (last_packet == 0)) last_packet = now ;


		signal Receive.receive(msg,payload,len-sizeof(rm_stat_t)) ;
		return msg ;
	}


	event void GeneralTimer.fired(){
		uint32_t time_to_next_fired ;
		int temp_drift = 0;

#ifdef TOSSIM
		uint32_t partial_modulo ;
		uint32_t fdrift = 0 ;
		uint32_t vdrift = 0 ;
#endif

//		printf("General Timer fired - state: %u\n",localState.proto_state) ;
		dbg("Manager","Timer fired! time: %s\n",sim_time_string()) ;
		switch(localState.proto_state){
			case PROTOSTATE_SLEEP:
//				printf("PROTOSTATE_SLEEP -> PROTOSTATE_GUARDTIME - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
				power_on_time = call GeneralTimer.getNow() ;
				call AdaptiveLayer.activePeriodBegin() ;

				if(call AdaptiveLayer.isResyncScheduled()/*call AdaptiveLayer.activePeriodBegin() == 0*/){
					need_resync = TRUE ; // must be called at the beginning of each sampling period
//					printf("Need resync\n") ;
					scheduleInfo.current_schedule_gt = call AdaptiveLayer.getCurrentGT() ;
					scheduleInfo.current_schedule_ri = call AdaptiveLayer.getCurrentRI() ;
					scheduleInfo.current_schedule_dci = call AdaptiveLayer.getCurrentDCI() ;
					scheduleInfo.current_schedule_skip = call AdaptiveLayer.getCurrentSkip() ;
					/*
					nx_uint16_t current_sampling_period  ;		// 2 -> I'm not root -> updates during IBC only
					nx_uint16_t next_sampling_period ;			// 2 -> I'm not root -> updates during IBC only
					nx_uint32_t time_to_next_sampling_period ;  // 4 -> WARNING -> need to check if I need to set it or not.
					nx_uint32_t next_sampling_period_fired ;	// 4 -> updated little bit later in this case (only if resync)
					*/
					call RMControl.reset() ;
				}
				else{
//					printf("No resync\n") ;
					call RMControl.pause() ;
				}

				time_to_next_fired = call ScheduleManagement.getCurrentSamplingPeriodMilli() ;

#ifdef TOSSIM
				/*
				 * Drift simulation in TOSSIM
				 * Two dirfts are considered: a fixed one which is randomly choosen between 0 and 49 ppm (hardware drift)
				 * and a variable one which is computed each time and varies between 0 and 9 ppm. Each drift has associated a boolean value
				 * which determines whether the drift must be added or subtracted from the timer (it is fixed for the fixed drift while
				 * it might change for the variable drift).
				 */
				if(/*!need_resync*/ ! call AdaptiveLayer.isResyncScheduled()){
					partial_modulo = call ScheduleManagement.getCurrentSamplingPeriodMilli() ;
					variable_drift = call Random.rand32() % (10UL) ;
					variable_drift_to_add = call Random.rand16() % 2 ;
					dbg("Manager-Drift","TOSSIM DRIFT - time_to_next_fired: %lu\n",time_to_next_fired) ;

					fdrift = fixed_drift*partial_modulo/10000UL ;
					dbg("Manager-Drift","TOSSIM DRIFT - temp_fdrift: %lu\n",fdrift) ;
					if(fdrift % 100 >= 50) fdrift = fdrift/100UL + 1 ;
					else fdrift = fdrift/100UL ;
					dbg("Manager-Drift","TOSSIM DRIFT - fdrift: %lu\n",fdrift) ;


					vdrift = variable_drift*partial_modulo/10000UL ;
					dbg("Manager-Drift","TOSSIM DRIFT - temp_vdrift: %lu\n",vdrift) ;
					if(vdrift % 100 >= 50) vdrift = vdrift/100UL + 1 ;
					else vdrift = vdrift/100UL ;
					dbg("Manager-Drift","TOSSIM DRIFT - vdrift: %lu\n",vdrift) ;


					if(fixed_drift_to_add){
						time_to_next_fired += fdrift ;
						if(variable_drift_to_add){
							time_to_next_fired += vdrift ;
						}
						else{
							time_to_next_fired -= vdrift ;
						}
					}
					else{
						time_to_next_fired -= fdrift ;
						if(variable_drift_to_add){
							time_to_next_fired += vdrift ;
						}
						else{
							time_to_next_fired -= vdrift ;
						}
					}
					dbg("Manager-Drift","TOSSIM DRIFT - adjusted time_to_next_fired: %lu\n",time_to_next_fired) ;
				}

#endif


				call GeneralTimer.startOneShot(time_to_next_fired) ; // default, use next_wakeup
				if(need_resync) scheduleInfo.next_sampling_period_fired = call GeneralTimer.getNow()+time_to_next_fired+(call ScheduleManagement.getCurrentSamplingPeriodMilli())*(call AdaptiveLayer.getCurrentSkip());


#ifndef TOSSIM
//				call LowPowerListening.setLocalDutyCycle(10000) ;
				call LowPowerListening.setLocalWakeupInterval(0) ;
#endif
				change_state(PROTOSTATE_GUARDTIME) ;
				if (call RoutingControl.start() != SUCCESS) fatal_problem(FE_PSS_ROUTINGNOSTART) ;
				call ProtocolTimer.startOneShot(call AdaptiveLayer.getCurrentGT()) ;
//				printf("Proto Timer started, will fire in: %u\n",call AdaptiveLayer.getCurrentGT()) ;
				break;

			case ROOTSTATE_PAUSED:
//				printf("ROOTSTATE_PAUSED -> ROOTSTATE_GT - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
				dbg("Manager","Was ROOTSTATE_PAUSED, now ROOTSTATE_GT, time: %s\n",sim_time_string()) ;
				change_state(ROOTSTATE_GT) ;
				call AdaptiveLayer.activePeriodBegin() ;
				if(call AdaptiveLayer.isResyncScheduled()){ // resync phase!

					// aggiunta
					call ScheduleManagement.switchToNextSamplingPeriod() ;

					dbg("DEBUG_TTRD","Resync phase - ttr: %u , ttrd: %u\n",ttr,ttrd) ;
					dbg("Manager-Stat-Max-Ttr","MaxTtr: %u\n",ttr) ;
					dbg("Manager-Stat-Max-Ttrd","MaxTtrd: %u\n",ttrd) ;
					dbg("Stat_TTR","%u\n",ttr);
					dbg("Stat_TTRD","%u\n",ttrd);
					dbg("Stat_PTSI","%u\n",call AdaptiveLayer.getCurrentDCI());
					dbg("Stat_RI","%u\n",call AdaptiveLayer.getCurrentRI());
					dbg("Stat_GT","%u\n",call AdaptiveLayer.getCurrentGT()) ;
					dbg("Stat_SKIP","%u\n",call AdaptiveLayer.getCurrentSkip()) ;

					scheduleInfo.current_schedule_gt = call AdaptiveLayer.getCurrentGT() ;
					scheduleInfo.current_schedule_ri = call AdaptiveLayer.getCurrentRI() ;
					scheduleInfo.current_schedule_dci = call AdaptiveLayer.getCurrentDCI() ;
					scheduleInfo.current_schedule_skip = call AdaptiveLayer.getCurrentSkip() ;
					/*
					nx_uint16_t current_sampling_period  ;		// 2 -> done just before
					nx_uint16_t next_sampling_period ;			// 2 -> update a little bit later, if resync scheduled only
					nx_uint32_t time_to_next_sampling_period ;  // 4 -> WARNING -> need to check if I need to set it or not.
					nx_uint32_t next_sampling_period_fired ;	// 4 -> updated little bit later in this case (only if resync)
					*/

//					printf("GT: %u, RI: %u, PTSI: %u, SKIP: %u\n",call AdaptiveLayer.getCurrentGT(),call AdaptiveLayer.getCurrentRI(),call AdaptiveLayer.getCurrentDCI(),call AdaptiveLayer.getCurrentSkip()) ;
					ttr = ttrd = 0 ;
					temp_drift = call NtpSynchronization.getNtpDrift() ;
					if(call NtpSynchronization.getScheduleDrift() != 0) time_to_next_fired = call NtpSynchronization.getScheduleDrift() - call NtpSynchronization.getNtpDrift() ;
					else time_to_next_fired = call ScheduleManagement.getCurrentSamplingPeriodMilli() - call NtpSynchronization.getNtpDrift() - call NtpSynchronization.getScheduleDrift() ;

//					call RMSynchronization.setBoostStartup(TRUE) ; was here in august
					call CtpInfo.triggerImmediateRouteUpdate() ;
#ifndef TOSSIM
//					printfflush() ;
#endif

				}
				else time_to_next_fired = call ScheduleManagement.getCurrentSamplingPeriodMilli() ;


				// Start timers
				call GeneralTimer.startOneShot(time_to_next_fired) ;

				if(call AdaptiveLayer.isResyncScheduled()){
					scheduleInfo.next_sampling_period_fired = call GeneralTimer.getNow()+time_to_next_fired+(call ScheduleManagement.getCurrentSamplingPeriodMilli())*(call AdaptiveLayer.getCurrentSkip());
					call NtpSynchronization.resetFields() ;

				}

				call ProtocolTimer.startOneShot((uint32_t)call AdaptiveLayer.getCurrentGT()) ;

				// Activate CTP
				if(call RoutingControl.start() != SUCCESS) fatal_problem(FE_RSP_ROUTINGNOSTART) ;

				dbg("Manager-Stat-AdaptiveLayer","Guardtime: %u , RI: %u , PTSI: %u\n",call AdaptiveLayer.getCurrentGT(),call AdaptiveLayer.getCurrentRI(),call AdaptiveLayer.getCurrentDCI()) ;
				break ;

			default:
				fatal_problem(FE_GENERALTIMER_UNKOWNSTATE) ;
				break;

			}
	}

	event void AdaptiveLayer.needInValuesUpdate(){
//		printf("Received update request from AE\n") ;
		if(call RootControl.isRoot()){
			call AdaptiveLayer.updateInValues(ttr,ttrd,call ScheduleManagement.getNextSamplingPeriod()) ;
		}
		// else do nothing
#ifndef TOSSIM
//		printfflush() ;
#endif
	}

//#ifndef TOSSIM debug only was here in august
//	event void Notify.notify(button_state_t state){
//		// new boot
//		if(state == BUTTON_PRESSED){
//
//			//		  if(!isInSync(localState.proto_state)){
//			//			  if(localState.proto_state == BOOT_MANUAL){
//			//				  change_state(PROTOSTATE_LPL_TRY_RESYNC) ;
//			//				  call LplResync.beginResyncProcess() ;
//			//			  }
//			//		  }
//			if(call RootControl.isRoot()){
//				call Reboot.propagate(2) ;
//			}
//			else{
//				lplMsg_t* pkt = call LplStateSender.getPayload(&msg_state_buffer,sizeof(lplMsg_t)) ;
//				pkt->type = localState.proto_state ;	// aggiunta rispetto a scheduleInfo
//				pkt->current_sampling_period = call ScheduleManagement.getCurrentSamplingPeriod() ;		// 2
//				pkt->current_schedule_gt = call AdaptiveLayer.getCurrentGT() ;			// 2
//				pkt->next_schedule_ri = call AdaptiveLayer.getNextRI() ;			// 2
//				pkt->current_schedule_dci = call AdaptiveLayer.getCurrentDCI() ;			// 2
//				pkt->current_schedule_skip = call AdaptiveLayer.getCurrentSkip() ;			// 1
//				pkt->next_sampling_period = localState.previous_state ;			// 2
//				pkt->time_to_next_sampling_period = localState.nextWakeUpTime - call GeneralTimer.getNow()  ;  // 4
//				pkt->next_sampling_period_fired = 0;	// 4
//
//				call LowPowerListening.setRxDutyCycle(&msg_state_buffer,10000) ;
//				if(call LplStateSender.send(AM_BROADCAST_ADDR,&msg_state_buffer,sizeof(lplMsg_t)) != SUCCESS){
//					call Leds.led0Toggle() ;
//				}
//			}
//		}
//	}
//
//	event void LplStateSender.sendDone(message_t* msg, error_t error){
//		if(error!= SUCCESS) call Leds.led0Toggle() ;
//		else call Leds.led1Toggle() ;
//	}
//#endif

	event void ProtocolTimer.fired(){

	  dbg("Manager","ProtocolTimer.fired(), state: %u , time: %s\n",localState.proto_state,sim_time_string());
	  switch(localState.proto_state){

		  case PROTOSTATE_GUARDTIME:
			  if(call AdaptiveLayer.isResyncScheduled()){ // if resynchronization is required, change state in treesetup and wait for a resync message
				call ProtocolTimer.startOneShot(call AdaptiveLayer.getCurrentRI()) ;
				change_state(PROTOSTATE_CTPTREESETUP) ;
//				printf("PROTOSTATE_GUARDTIME -> PROTOSTATE_CTPTREESETUP - Need resync - Tchk: %lu, will fire in: %u\n",call GeneralTimer.getNow(),call AdaptiveLayer.getCurrentRI()) ; // was enabled in august
				dbg("Manager","Was PROTOSTATE_GUARDTIME, now PROTOSTATE_CTPTREESETUP need resync, time: %s\n",sim_time_string());
				call CtpInfo.triggerImmediateRouteUpdate() ; //not useful since it already done in guardtime
			  }
			  else{ // do not need to resync, can send the packet directly in the prepare_to_sleep interval
				call ProtocolTimer.startOneShot(call AdaptiveLayer.getCurrentDCI()) ;
				dbg("Manager","PrepareToSleep from AL: %u\n",call AdaptiveLayer.getCurrentDCI()) ;
				change_state(PROTOSTATE_PREPARETOSLEEP) ;
//				printf("PROTOSTATE_GUARDTIME -> PROTOSTATE_PREPARETOSLEEP - No resync - Tchk: %lu\n",call GeneralTimer.getNow()) ;// was enabled in august
				dbg("Manager","Was PROTOSTATE_GUARDTIME, now PROTOSTATE_PREPARETOSLEEP no resync, time: %s\n",sim_time_string());

				if(msg_len!=0)sendPacket() ;

			  }
			  break;

		  case PROTOSTATE_CTPTREESETUP: // go in out_of_sync and trigger the lpl manager
			  if(call AdaptiveLayer.isResyncScheduled() && need_resync){
				  need_resync = FALSE ;
				  if(call RoutingControl.stop() != SUCCESS){
					  fatal_problem(FE_PROTOTIMER_CTS_ROUTINGNOSTOP) ;
					  return ;
				  }
				  call ProtocolTimer.stop() ;
				  call GeneralTimer.stop() ;
				  change_state(PROTOSTATE_LPL_TRY_RESYNC) ;
//				  printf("PROTOSTATE_CTPTREESETUP -> PROTOSTATE_LPL_TRY_RESYNC - Starting OOS Manager - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
				  signal DISSenseSampling.syncNotification(FALSE) ;
				  call LplResync.beginResyncProcess() ;
				  dbg("Manager","Was PROTOSTATE_CTPTREESETUP, now PROTOSTATE_LPL_TRY_RESYNC, time: %s\n",sim_time_string());
			  }
			  else{ // actually, I should check if a resync was scheduled... but it should be fine also like this.
				  dbg("Manager","Was PROTOSTATE_CTPTREESETUP, now PROTOSTATE_PREPARETOSLEEP, time: %s\n",sim_time_string());
				  change_state(PROTOSTATE_PREPARETOSLEEP) ;
//				  printf("PROTOSTATE_CTPTREESETUP -> PROTOSTATE_PREPARETOSLEEP - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
				  call ProtocolTimer.startOneShot(call AdaptiveLayer.getCurrentDCI()) ;
				  if(msg_len!=0)sendPacket() ;
			  }
			  break;

		  case PROTOSTATE_PREPARETOSLEEP:

				goto_sleep();

				// resets stats
				power_on_time = 0 ;
				resync_time_elapsed = 0 ;
//				printf("PROTOSTATE_PREPARETOSLEEP -> PROTOSTATE_SLEEP - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
				dbg("Manager","Was PROTOSTATE_PREPARETOSLEEP, now PROTOSTATE_SLEEP, time: %s\n",sim_time_string());

			break;

		  // ROOT STATE: can be only active
		  case ROOTSTATE_GT:
//			  printf("ROOTSTATE_GT -> ROOTSTATE_ACTIVE - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
			  dbg("Manager","Was ROOTSTATE_GT, now ROOTSTATE_ACTIVE, time: %s\n",sim_time_string()) ;
			  change_state(ROOTSTATE_ACTIVE) ;
//			  call RMSynchronization.setBoostStartup(FALSE) ; was here in august
			  dbg("AdaptiveLayer","Interval ri: %u , interval_ptsi: %u\n",(uint32_t)call AdaptiveLayer.getCurrentRI(),(uint32_t)call AdaptiveLayer.getCurrentDCI()) ;
			  if(call AdaptiveLayer.isResyncScheduled()){
				  call ProtocolTimer.startOneShot((uint32_t)call AdaptiveLayer.getCurrentRI()+(uint32_t)call AdaptiveLayer.getCurrentDCI()) ;
			  }
			  else{
				  call ProtocolTimer.startOneShot((uint32_t)call AdaptiveLayer.getCurrentDCI()) ;
			  }
			  break;
		  case ROOTSTATE_ACTIVE:
//			  printf("ROOTSTATE_ACTIVE -> ROOTSTATE_PAUSED - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
			  dbg("Manager","Was ROOTSTATE_ACTIVE, now ROOTSTATE_PAUSED, time: %s\n",sim_time_string()) ;
			  if(call RoutingControl.stop() != SUCCESS) fatal_problem(FE_PROTOTIMER_RSA_ROUTINGNOSTOP) ;

			  // stats for adaptive layer:
			  if((last_packet - first_packet) > 0){
				  if(ttrd == 0){
					  dbg("DEBUG_TTRD","ttrd = 0\n") ;
					  ttrd = last_packet - first_packet ;
				  }
				  dbg("DEBUG_TTRD","Single ttrd : %u - (%s)\n",last_packet - first_packet,sim_time_string()) ;
				  if(ttrd < (last_packet - first_packet)) ttrd = (RISING_ALPHA*ttrd + (10-RISING_ALPHA)*(last_packet - first_packet)) / 10 ;
				  else ttrd = (FALLING_ALPHA*ttrd + (10-FALLING_ALPHA)*(last_packet - first_packet)) / 10 ;
				  dbg("DEBUG_TTRD","Total ttrd: %u - (%s)\n",ttrd,sim_time_string()) ;
//				  printf("Single first: %lu, last: %lu, ttrd: %lu, ttrd_tot: %u\n",first_packet,last_packet,(last_packet-first_packet),ttrd) ; // was enabled in august
//				  printfflush() ; // was enabled in august
			  }



			  first_packet = 0 ;
			  last_packet = 0 ;

			  change_state(ROOTSTATE_PAUSED) ;
			  break;


		  default:

			  fatal_problem(FE_PROTOTIMER_UNKOWNSTATE) ;
			  break;

	  }
#ifndef TOSSIM
//	  printfflush() ; // was enabled in august
#endif
	}


	/*
	 * New method for the interface: instead of using just one method (isSafeInterval), the interface will use one method for each situation:
	 * 	- Safe interval to Transmit LPLState Msg
	 *  - Safe interval to transmit LPLSyncReply method
	 *  - Safe interval to transmit LPLSyncRequest method
	 *  - Safe interval to receive LPLStateMsg
	 *  - Safe interval to receive LPLSyncReply method
	 *  - Sage interval to receive LPLSyncResponse method
	 *
	 * Output:
	 * 	- to be defined.
	 */

//	/*
//	 * Received an LPLState request. The node can reply only if:
//	 * 	- It is OUT_OF_SYNC and it is not trying to recynchronize nor it is replying to a previous state request.
//	 *  - It is IN_SYNC and it is in SLEEP mode
//	 * If one of these two conditions is fit, then reply with TRUE and atomically change the state of the
//	 *  node to ....(tbd)... otherwise return FALSE.
//	 */
//	command bool ProtocolScheduler.allowLplStateResponse(){
////		atomic{
////			uint32_t value = 6094UL ; // to be defined
////			if(isInSync(localState.proto_state)){
////				if(/*call RMScheduler.getSelfNextWakeUp()*/ call ScheduleManagement.timeToNextWakeUp() > value && (localState.proto_state == PROTOSTATE_SLEEP || localState.proto_state == ROOTSTATE_PAUSED)){
////					change_state(PROTOSTATE_STATE_RESP_SENDING) ; // check for sink if it requires a particular state
////					return TRUE ;
////				}
////				else return FALSE ;
////			}
////			else{
////				if(localState.proto_state == PROTOSTATE_LPL_TRY_RESYNC){
////					change_state(PROTOSTATE_LPL_STATE_RESP_SENDING) ;
////				}
////				else return FALSE ;
////			}
////		}
//		return FALSE ; // no more implemented
//
//	}


	/*
	 * Received an LPLSync request. The can reply only if:
	 * 	- I am IN_SYNC AND I am in SLEEP mode AND there is enough time to next wakeup
	 */
	command bool ProtocolScheduler.allowLplSyncResponse(uint16_t value){
		atomic{
			if(isInSync(localState.proto_state)){ // for root I need to check that the net is in sleep state
				if(call ScheduleManagement.timeToNextWakeUp() > (uint32_t)value && (localState.proto_state == PROTOSTATE_SLEEP || localState.proto_state == ROOTSTATE_PAUSED)){
					change_state(PROTOSTATE_SYNC_RESP_SENDING) ; // check for sink if it requires a particular state
					return TRUE ;
				}
				else return FALSE ;
			}
			else return FALSE ;
		}

	}


	/*
	 * I'm out of sync, need to tx a sync request
	 */
	command bool ProtocolScheduler.allowLplSyncRequest(){
		atomic{
			if(isInSync(localState.proto_state)){
				return FALSE ;
			}
			else{
				if(localState.proto_state == PROTOSTATE_LPL_TRY_RESYNC || localState.proto_state == PROTOSTATE_LPL_SYNC_RESP_WAITING){
					change_state(PROTOSTATE_LPL_SYNC_REQ_SENDING) ;
					return TRUE ;
				}
				else return FALSE ;
			}
		}

	}



//	command void ProtocolScheduler.stateResponseSendDone(){
////		atomic{
////			if((localState.proto_state == PROTOSTATE_STATE_RESP_SENDING) || (localState.proto_state == PROTOSTATE_LPL_STATE_RESP_SENDING)){
////				restore_previous_state() ;
////			}
////			else fatal_problem(FE_STATERESPONSESENDDONE_UNKOWNSTATE) ;
////		}
//		// No more implemented
//	}

	command void ProtocolScheduler.syncResponseSendDone(){
		atomic{
			if(localState.proto_state == PROTOSTATE_SYNC_RESP_SENDING){
				restore_previous_state() ;
			}
			else fatal_problem(FE_SYNCRESPONSESENDDONE_UNKOWNSTATE) ;
		}
	}

	command void ProtocolScheduler.syncRequestSendDone(){
		atomic{
			if(!isInSync(localState.proto_state)){ // I might have already receive a resync from somebody else
				if(localState.proto_state == PROTOSTATE_LPL_SYNC_REQ_SENDING){
					change_state(PROTOSTATE_LPL_SYNC_RESP_WAITING) ;
#ifndef TOSSIM
//					call LowPowerListening.setLocalDutyCycle(10000) ;
					call LowPowerListening.setLocalWakeupInterval(0) ;
#endif
					call SyncWaitTimer.startOneShot(/*call LowPowerListening.dutyCycleToSleepInterval(10)*/4463+LPL_RESPONSE_BACKOFF) ;
				}
				else fatal_problem(FE_SYNCREQUESTSENDDONE_UNKNWOWNSTATE) ;
			}
		}
	}

	event void SyncWaitTimer.fired(){
		if(!isInSync(localState.proto_state)){
			if(localState.proto_state == PROTOSTATE_LPL_SYNC_RESP_WAITING){
#ifndef TOSSIM
//					call LowPowerListening.setLocalDutyCycle(10) ;
				call LowPowerListening.setLocalWakeupInterval(4463) ;
#endif
			}
		}
	}

	event void RMSynchronization.reSync(){

		uint32_t resync_nwu ;
		uint32_t resync_time = call GeneralTimer.getNow() ;
//		uint32_t remaining = 0 ;
#ifdef TOSSIM
		uint32_t partial_modulo ;
		uint32_t fdrift = 0 ;
		uint32_t vdrift = 0 ;
		variable_resync_drift = (uint8_t) (call Random.rand16() % 5) ;
#endif

		resync_time_elapsed = (resync_time > power_on_time)? (resync_time - power_on_time) : (-1U - power_on_time + resync_time) ;
//		printf("Resynchronized! - Tchk: %lu\n",call GeneralTimer.getNow()) ; // was enabled in august
		dbg("Manager","Resync!, time: %s\n",sim_time_string());

		switch(localState.proto_state){

		  case PROTOSTATE_GUARDTIME: case PROTOSTATE_CTPTREESETUP:
			  if(need_resync){
//			  Resynchronization....
			  atomic{
				resync_nwu = localState.nextWakeUpTime - call GeneralTimer.getNow() ;
#ifdef TOSSIM
				dbg("Manager-Drift","TOSSIM DRIFT - first rnwu: %lu\n",resync_nwu) ;
				resync_nwu += variable_resync_drift ;
				dbg("Manager-Drift","TOSSIM DRIFT - adjusted (vrd) rnwu: %lu\n",resync_nwu) ;
#endif
#ifdef TOSSIM
				/*
				 * Drift simulation in TOSSIM
				 * Two dirfts are considered: a fixed one which is randomly choosen between 0 and 49 ppm (hardware drift)
				 * and a variable one which is computed each time and varies between 0 and 9 ppm. Each drift has associated a boolean value
				 * which determines whether the drift must be added or subtracted from the timer (it is fixed for the fixed drift while
				 * it might change for the variable drift).
				 */
				partial_modulo = call ScheduleManagement.getCurrentSamplingPeriodMilli() ;
				variable_drift = call Random.rand32() % (10UL) ;
				variable_drift_to_add = call Random.rand16() % 2 ;
				dbg("Manager-Drift","TOSSIM DRIFT - rnwu: %lu\n",resync_nwu) ;

				fdrift = fixed_drift*partial_modulo/10000UL ;
				dbg("Manager-Drift","TOSSIM DRIFT - temp_fdrift: %lu\n",fdrift) ;
				if(fdrift % 100 >= 50) fdrift = fdrift/100UL + 1 ;
				else fdrift = fdrift/100UL ;
				dbg("Manager-Drift","TOSSIM DRIFT - fdrift: %lu\n",fdrift) ;


				vdrift = variable_drift*partial_modulo/10000UL ;
				dbg("Manager-Drift","TOSSIM DRIFT - temp_vdrift: %lu\n",vdrift) ;
				if(vdrift % 100 >= 50) vdrift = vdrift/100UL + 1 ;
				else vdrift = vdrift/100UL ;
				dbg("Manager-Drift","TOSSIM DRIFT - vdrift: %lu\n",vdrift) ;


				if(fixed_drift_to_add){
					resync_nwu += fdrift ;
					if(variable_drift_to_add){
						resync_nwu += vdrift ;
					}
					else{
						resync_nwu -= vdrift ;
					}
				}
				else{
					resync_nwu -= fdrift ;
					if(variable_drift_to_add){
						resync_nwu += vdrift ;
					}
					else{
						resync_nwu -= vdrift ;
					}
				}
				dbg("Manager-Drift","TOSSIM DRIFT - adjusted rnwu: %lu\n",resync_nwu) ;
//				dbg("Manager","TOSSIM DRIFT - rnwu: %lu , fixed_drift: %lu , fixed_to_add: %u , variable_drift: %lu , variable_to_add: %u\n",resync_nwu,fixed_drift*partial_modulo/100000UL,fixed_drift_to_add,variable_drift*partial_modulo/100000UL,variable_drift_to_add) ;


#endif




				if(call GeneralTimer.isRunning()) call GeneralTimer.stop() ;
				call GeneralTimer.startOneShot(resync_nwu) ;
				if(sampling_offset != 0 && sampling_offset < resync_nwu) call DISSenseSamplingTimer.startOneShot(resync_nwu-sampling_offset) ;
				scheduleInfo.next_sampling_period_fired = call GeneralTimer.getNow()+call GeneralTimer.getdt()+(call ScheduleManagement.getCurrentSamplingPeriodMilli())*(call AdaptiveLayer.getCurrentSkip());
				scheduleInfo.current_schedule_gt = call AdaptiveLayer.getCurrentGT() ;
				scheduleInfo.current_schedule_ri = call AdaptiveLayer.getCurrentRI() ;
				scheduleInfo.current_schedule_dci = call AdaptiveLayer.getCurrentDCI() ;
				scheduleInfo.current_schedule_skip = call AdaptiveLayer.getCurrentSkip() ;
				/*
				nx_uint16_t current_sampling_period  ;		// 2 -> WARNING -> need to check if I should do something here...
				nx_uint16_t next_sampling_period ;			// 2 -> WARNING -> need to check if I should do something here...
				nx_uint32_t time_to_next_sampling_period ;  // 4 -> WARNING -> need to check if I need to set it or not.
				*/
				call CtpInfo.triggerImmediateRouteUpdate() ;
				need_resync = FALSE ;
				signal DISSenseSampling.syncNotification(TRUE) ;
			  }



//			  Why should I turn in tree setup? isn't it better to remain in GT?
//				  if(localState.proto_state == PROTOSTATE_GUARDTIME){
//					  if(call ProtocolTimer.isRunning()){
//						  call ProtocolTimer.stop() ;
//						  remaining = ((call ProtocolTimer.gett0() + call ProtocolTimer.getdt()) - (call ProtocolTimer.getNow()));
//						  remaining += call AdaptiveLayer.getCurrentRI() ;
//						  change_state(PROTOSTATE_CTPTREESETUP) ;
//						  call ProtocolTimer.startOneShot(remaining) ;
//					  }
//				  }

			  }
			  else fatal_problem(RX_SYNC_BUT_WAS_ALREADY) ;

			  break;

		  // This condition should never happen since when I'm in try resync the routing layer is stopped... However it happened once during tests -> need to investigate.
		  case PROTOSTATE_LPL_SYNC_REQ_SENDING: case PROTOSTATE_LPL_SYNC_RESP_WAITING: /*case PROTOSTATE_LPL_STATE_RESP_SENDING:*/
			  // do nothing
			  call Leds.led2Toggle() ;
			  break;


		  // faulty states
		  default:
			  dbg("Manager","Faulty state: %u\n",localState.proto_state) ;
			  fatal_problem(FE_RESYNCEVNT_UNKOWNSTATE) ;
			  break;
	  }
#ifndef TOSSIM
//	  printfflush() ; // was enabled in august
#endif
	}


	/*
	 * Resync if:
	 * 	- Booting in manual mode
	 *  - Waiting for a resync message
	 *  - Is trying to resync
	 *
	 */

	event void LplResync.receivedSync(lplMsg_t* lplmsgRx){
//		uint32_t next_resync_time ;
//			printf("Received resync!\n") ;
			dbg("Manager","Received resync!\n") ;
			if(/*localState.proto_state == BOOT_MANUAL ||*/ localState.proto_state == PROTOSTATE_LPL_SYNC_RESP_WAITING || localState.proto_state == PROTOSTATE_LPL_TRY_RESYNC || localState.proto_state == PROTOSTATE_LPL_SYNC_REQ_SENDING){
				resync_counter++ ;
				dbg("Manager","Type: %u Interval: %u NextWU: %lu\n",lplmsgRx->type,lplmsgRx->current_sampling_period,lplmsgRx->time_to_next_sampling_period) ;
//				printf("Resync message:\n Current S.p.: %u\n GT: %u\n RI: %u\n DCI: %u\n Skip: %u\n Next S.p.: %u\n Next fired: %lu\n TFired: %lu\n",lplmsgRx->current_sampling_period,lplmsgRx->current_schedule_gt,lplmsgRx->current_schedule_ri,lplmsgRx->current_schedule_dci,lplmsgRx->current_schedule_skip,lplmsgRx->next_sampling_period,lplmsgRx->time_to_next_sampling_period,lplmsgRx->next_sampling_period_fired) ;

//				next_resync_time = lplmsgRx->time_to_next_sampling_period ;
				call GeneralTimer.startOneShot(lplmsgRx->schedule_info.time_to_next_sampling_period) ;
				if(sampling_offset != 0 && sampling_offset < lplmsgRx->schedule_info.time_to_next_sampling_period) call DISSenseSamplingTimer.startOneShot(lplmsgRx->schedule_info.time_to_next_sampling_period-sampling_offset) ;
				call ScheduleManagement.resyncNextWakeUp(lplmsgRx->schedule_info.time_to_next_sampling_period) ; // typically used by RE to set the resync.
				call ScheduleManagement.setCurrentSamplingPeriod(lplmsgRx->schedule_info.current_sampling_period) ;
				call ScheduleManagement.setNextSamplingPeriod(lplmsgRx->schedule_info.next_sampling_period) ;
				scheduleInfo.next_sampling_period_fired = lplmsgRx->schedule_info.time_to_next_sampling_period + call GeneralTimer.getNow() ;

				// need to add schedule info updates!

				// Need to wait for the first resync period before entering in the network...
				showResynchronized() ;

//				printf("Will wakeup in: %lu\n",next_resync_time) ;



				// initialize the adaptive layer with the new values... (cntdwn_to_resync must be 1 such that it is reset during the wakeup)
				call AdaptiveLayer.initialize(1,CTPTREESETUP_TIMEOUT,PREPARETOSLEEP_TIMEOUT,lplmsgRx->schedule_info.current_sampling_period,lplmsgRx->schedule_info.current_schedule_ri,lplmsgRx->schedule_info.current_schedule_dci,lplmsgRx->schedule_info.current_schedule_skip) ;

				// stops try resync:
				call LplResync.stopResyncProcess() ;


				signal DISSenseSampling.syncNotification(TRUE) ;
				goto_sleep() ;

			}
			else{
				if(isInSync(localState.proto_state)){
					dbg("Manager","Already in sync. Ignoring...\n");
//					printf("Already in sync. Ignoring...\n") ; // was enabled in august
				}
				else{
					dbg("Manager","Unexpected state: %u\n",localState.proto_state) ;

					fatal_problem(FE_RECEIVEDSYNC_UNKOWNSTATE) ; // This event is generated sometimes when I resync the node.
				}
			}
#ifndef TOSSIM
//			printfflush() ; // was enabled in august
#endif
	}

	event void DISSenseSamplingTimer.fired(){
		signal DISSenseSampling.startSampling() ;
	}

	static void sendPacket(){
		error_t err ;
		rm_stat_t* rmf ;
		if(msg_len != 0){

			/* DEBUG PURPOSE ONLY */
#ifdef DISSENSEDEBUG
			// DISABLED FOR DEMO CATANIA -> sensor 4 and 5 are affected by forwarding engine only.
			/*
			app_data_t* in = call SubSend.getPayload(msg_buffer,msg_len) ;
			in->sensor[0].data = 0xff ; // ctp forwarding engine parent
			in->sensor[0].temp = 0 ; // ctp forwarding engine retries
			in->sensor[1].data = 0xff ; // ctp forwarding engine packet_queue_inpacket
			in->sensor[1].temp = 0xff ; // ctp forwarding engine packet_tx_fail_inpacket
			in->sensor[2].data = resync_counter ;
			in->sensor[2].temp = call ScheduleManagement.getCurrentSamplingPeriod() ;
			in->sensor[3].data = call AdaptiveLayer.getCurrentGT() ;
			in->sensor[3].temp = call AdaptiveLayer.getCurrentRI() ;
			in->sensor[4].data = call AdaptiveLayer.getCurrentDCI() ;
			in->sensor[4].temp = call AdaptiveLayer.getCurrentSkip() ;
*/
//			in->inQueue = 10 ; // FE field
//			in->oosCounter = resync_counter ;
//			in->failed = 11 ;// FE field
//			in->samplingPeriod = call ScheduleManagement.getCurrentSamplingPeriod() ;
//			in->gt = call AdaptiveLayer.getCurrentGT() ;
//			in->ri = call AdaptiveLayer.getCurrentRI() ;
//			in->dci = call AdaptiveLayer.getCurrentDCI() ;
//			in->skip = call AdaptiveLayer.getCurrentSkip() ;
			/*                    */
#endif
			rmf = (rm_stat_t*) (call SubSend.getPayload(msg_buffer,msg_len+sizeof(rm_stat_t)) + msg_len) ;
			rmf->ttr = (resync_time_elapsed != 0)? (uint16_t) resync_time_elapsed : 0xffff;
			err = call SubSend.send(msg_buffer, msg_len+sizeof(rm_stat_t)) ;
			if (err == SUCCESS)
			  sendbusy = TRUE;
			else{
			  msg_len = 0 ;
			  signal Send.sendDone(msg_buffer,err) ;
			}
		}
	}

	static void change_state(uint8_t state){

		atomic{
//		  state_transition_debug_t tr ;
		  localState.previous_state = localState.proto_state ;
		  localState.proto_state = state ;
//		  tr.old_state = localState.previous_state ;
//		  tr.new_state = localState.proto_state ;
//		  tr.timestamp = call GeneralTimer.getNow() ;
//		  if(call StateQueue.size() == call StateQueue.maxSize()){
//			  call StateQueue.dequeue() ;
//		  }
//		  call StateQueue.enqueue(tr) ;
		}
	}

	static void restore_previous_state(){
		atomic{
			uint8_t state = localState.proto_state ;
			localState.proto_state = localState.previous_state ;
			localState.previous_state = state ;
		}
	}

	static bool isInSync(uint8_t state){
		  return (state & IN_SYNC) ;
	}

	static void fatal_problem(uint8_t errcode){
		dbg("Manager","FATAL PROBLEM! - %u\n",errcode) ;
//		printf("Fatal Problem! - %u\n",errcode) ;
		call GeneralTimer.stop() ;
		call ProtocolTimer.stop() ;
		call LplResync.stopResyncProcess() ;
		call Leds.led0On() ;
		call Leds.led1On() ;
		call Leds.led2On() ;
		change_state(errcode) ;
//		addCheckPoint(FATALPROBLEM_ERRCODE,(uint32_t)errcode) ;
#ifndef TOSSIM
//		printfflush();
#endif
	}

	static void goto_sleep(){
		  if(call RoutingControl.stop() != SUCCESS) fatal_problem(FE_GOTOSLEEP_ROUTINGNOSTOP) ;
#ifndef TOSSIM
//		  call LowPowerListening.setLocalDutyCycle(10) ;
		  call LowPowerListening.setLocalWakeupInterval(4463) ;
#endif

//		  addCheckPoint(GOTOSLEEP_MSGSENT,(uint32_t)msg_sent) ;
		  if(msg_sent){
//			  msg_len = 0 ;
			  msg_sent = FALSE ;
		  }
		  else{
			  //if(sendbusy){ // when resynchronizing I may not have tried to send a packet, so check disabled
//				  msg_len = 0 ;
				  msg_error = FAIL ;
				  sendbusy = FALSE ;
			  //}
		  }
		  change_state(PROTOSTATE_SLEEP) ;
		  if(msg_len != 0){ // it means that I must signal a sendDone
			  msg_len = 0 ;
			  signal Send.sendDone(msg_buffer,msg_error) ;
		  }
		  dbg("Manager","Sleeping, time: %s\n",sim_time_string()) ;
	}

//	static void addCheckPoint(uint8_t idx , uint32_t value){
//		check_point_t ch ;
//		ch.id = idx ;
//		ch.value = value ;
//		ch.timestamp = call GeneralTimer.getNow() ;
//		if(call CheckQueue.size() == call CheckQueue.maxSize()){
//			call CheckQueue.dequeue() ;
//		}
//		call CheckQueue.enqueue(ch) ;
//	}

//	static void printStateAndCheck(){
////		int i ;
////		printf("Checkpoints: ");
////		for(i=0;i<checkpoints_cnt;i++){
////			printf("%u ",checkpoints[i]) ;
////		}
////		printf("\n") ;
////		printfflush() ;
//		int i ;
//		for(i=0;i<call StateQueue.size();i++){
//			state_transition_debug_t st = call StateQueue.element(i) ;
//			printf("%u %u %u %lu\n",i,st.old_state,st.new_state,st.timestamp) ;
//#ifndef TOSSIM
//			if(i%5 == 4) printfflush() ;
//#endif
//		}
//#ifndef TOSSIM
//		printfflush() ;
//		printf("========================================\n") ;
//		printfflush() ;
//#endif
//		for(i=0;i<call CheckQueue.size();i++){
//			check_point_t ch = call CheckQueue.element(i) ;
//			printf("%u %u %lu %lu\n",i,ch.id,ch.value,ch.timestamp) ;
//#ifndef TOSSIM
//			if(i%5 == 4) printfflush() ;
//#endif
//		}
//#ifndef TOSSIM
//		printfflush() ;
//#endif
//
//	}

	static void showResynchronized(){
		call Leds.led0On() ;
		call ShowResyncTimer.startOneShot(1024U*10U) ;
	}


// interface ScheduleManagement
	command uint16_t ScheduleManagement.getCurrentSamplingPeriod(){ return scheduleInfo.current_sampling_period ;}
	command uint16_t ScheduleManagement.getCurrentGt() { return scheduleInfo.current_schedule_gt ;}
	command uint16_t ScheduleManagement.getCurrentRi() { return scheduleInfo.current_schedule_ri ;}
	command uint16_t ScheduleManagement.getCurrentDci(){ return scheduleInfo.current_schedule_dci ;}
	command uint8_t ScheduleManagement.getCurrentSkip() { return scheduleInfo.current_schedule_skip ;}
	command uint16_t ScheduleManagement.getNextSamplingPeriod() { return scheduleInfo.next_sampling_period ;}
	command uint32_t ScheduleManagement.getTimeToNextSP(){return scheduleInfo.next_sampling_period_fired - call GeneralTimer.getNow();}
	command uint32_t ScheduleManagement.getNextSamplingPeriodFired() { return scheduleInfo.next_sampling_period_fired ;}
	command uint16_t ScheduleManagement.getNextRi(){return call AdaptiveLayer.getNextRI();}


	command void ScheduleManagement.setNextSamplingPeriod(uint16_t nsp){
		scheduleInfo.next_sampling_period = nsp ;
	}

	command void ScheduleManagement.setCurrentSamplingPeriod(uint16_t csp){
		scheduleInfo.current_sampling_period = csp ;
	}

	command void ScheduleManagement.switchToNextSamplingPeriod(){
		scheduleInfo.current_sampling_period = scheduleInfo.next_sampling_period ;
	}

	command uint32_t ScheduleManagement.getCurrentSamplingPeriodMilli(){
		return 1024UL * (uint32_t) scheduleInfo.current_sampling_period ;
	}

	command uint32_t ScheduleManagement.getNextWakeUp(){
		return (call GeneralTimer.gett0() + call GeneralTimer.getdt()) ;
	}

	command void ScheduleManagement.resyncNextWakeUp(uint32_t nwut){
		if(nwut == 0) fatal_problem(FE_SETPARENTNEXTWAKEUPTIME) ; // must check the synchronization validity
		  dbg("RE_BUG_2","%s - nwut: %u\n",__FUNCTION__,nwut);
		  localState.nextWakeUpTime = nwut ;
	}

	command uint32_t ScheduleManagement.timeToNextWakeUp(){
		  return ((call GeneralTimer.gett0() + call GeneralTimer.getdt()) - (call GeneralTimer.getNow())) ;
	}

	command void DISSenseSampling.setSamplingOffset(uint32_t offset){
		sampling_offset = offset ;
	}

	command uint8_t DISSenseStatistics.getSkip(){
		return call ScheduleManagement.getCurrentSkip() ;
	}

	command uint16_t DISSenseStatistics.getGt(){
		return call ScheduleManagement.getCurrentGt() ;
	}

	command uint16_t DISSenseStatistics.getRi(){
		return call ScheduleManagement.getCurrentRi() ;
	}

	command uint16_t DISSenseStatistics.getDci(){
		return call ScheduleManagement.getCurrentDci() ;
	}

	command uint16_t DISSenseStatistics.getSP(){
		return call ScheduleManagement.getCurrentSamplingPeriod() ;
	}
}
